#include "LowPass.h"


LowPass::LowPass(int size)
{
	_size = size;
	avg = osg::Vec3d(0, 0, 0);
	n = 0;
	__avg = osg::Vec3d(0, 0, 0);
}


LowPass::~LowPass()
{
}
void LowPass::reset()
{
	filter.clear();
	avg = osg::Vec3d(0, 0, 0);
	__avg = osg::Vec3d(0, 0, 0);
	n = 0;

}
void LowPass::add(osg::Vec3d value)
{
	filter.push_back(value);
	avg += (value / _size);
	if (filter.size() > _size)
	{
		auto temp = filter.front();
		avg -= (temp / _size);
		filter.pop_front();
	}
}
osg::Vec3d LowPass::get()
{
	if (filter.size() == _size)
		return avg;
	return avg*((float)_size / (float)filter.size());
}	
osg::Vec3d LowPass::getAvg(osg::Vec3d value)
{
	int lastsize = n++;
	if (n == 1)
	{
		__avg = value;
	}
	else
	{
		__avg = __avg *(lastsize / (double)n) + value*(1 / (double)n);
	}
	return __avg;

}
